//#include "math.h"
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include <HC_SR04.h>

//debug variable
//when true robot should face "forward" in the starting position of the maze
bool frontFacing = true;

bool usesCm = false;

// Creates the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Select which 'port' M1, M2, M3 or M4.
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(3);  // M1 Front L
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(4);  // M2 Front R
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(1);  // M3 Rear L
Adafruit_DCMotor *myMotor4 = AFMS.getMotor(2);  // M4 Rear R

int NINETY_DEGREE_DELAY = 900;
int NINETY_DEGREE_SPEED = 128;
int FORWARD_3INCHES_DELAY = 250;
int FORWARD_1INCHES_DELAY = 70;
int FORWARD_3INCHES_SPEED = 128;
//the delay need to move either one cm or one inch
//defined in setup
int UNIT_DISTANCE_DELAY = -1;
int CM_DISTANCE_DELAY = 50; //TODO: Measure this 
int INCHES_DISTANCE_DELAY = 60; //TODO: Measure this
int maxWallDistance = -1;
int angle_slow_offset = -64;

//ultrasonic objects
HC_SR04 us1(30,31);
HC_SR04 us2(32,33);
HC_SR04 us3(34,35);
HC_SR04 us4(36,37);
HC_SR04 us5(38,39);
HC_SR04 us6(40,41);

long d1;
long d2;
long d3;
long diff23;
//when at a corner (meaning one sensor sees space and one doesn't)
//we record cornerWallDistance value so that the robot will clear that wall 
//when  it moves forward into the new space
long cornerWallDistance;
long WALL_REACHED_DISTANCE;
long ROBOT_LENGTH = 20/2.54; 
boolean cornerWallDetected = false;

//------------------------------------------------------------------------------------------
void setup(){
  Serial.begin(9600);
  AFMS.begin();
  uint8_t i;
  //motors_stop();
  //calibratePos();
  	
  delay(1000);
  Serial.println("5");
  delay(1000);
  Serial.println("4");
  delay(1000);
  Serial.println("3");
  delay(1000);
  Serial.println("2");
  delay(1000);
  Serial.println("1");
  delay(1000);
    
  if(usesCm){
    UNIT_DISTANCE_DELAY = CM_DISTANCE_DELAY;
    maxWallDistance = 30;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 5;
    ROBOT_LENGTH = 20; //TODO: Measure this
  }
  
  else{
    UNIT_DISTANCE_DELAY = INCHES_DISTANCE_DELAY;
    maxWallDistance = 30/2.54;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 4;
    ROBOT_LENGTH = 20/2.54; //TODO: Measure this
  }
  	
  if(frontFacing)
    turn_right(NINETY_DEGREE_DELAY, NINETY_DEGREE_SPEED);
}
//------------------------------------------------------------------------------------------
void loop(){        	
  Serial.println("NEW CYCLE");
  Serial.println();
  
  //////////////////////////
  //check if there is a wall immediately in front of the robot
  //////////////////////////
  d1 = us1.getDistance(usesCm);
        
  if(d1 < WALL_REACHED_DISTANCE){
    turn_left(NINETY_DEGREE_DELAY, NINETY_DEGREE_SPEED);
    return;
  }
  else{
    //move forward, but not too much
    // commenting out this forward to use if statements
    //motors_forward(FORWARD_3INCHES_DELAY,FORWARD_3INCHES_SPEED);
    
    //TODO: insert if statements for correction to get perpendicular to wall
    
    //compare sensor readings us2 and us3; diff23 is absolute value of difference
    //try to keep within 1/2 inch
    delay(60);
    d2 = us2.getDistance(usesCm);
    d3 = us3.getDistance(usesCm);
    diff23 = d2 - d3;
    diff23 = abs(diff23);
                
    // if the distance is greater than 2in and less than 5in, angle correct before moving forward
    if(diff23 >= 2 && diff23 < 8){
      Serial.println("calling angle_correction() before moving");
      angle_correction();
    }
                
    // if the distance is less than 2in with some offset
    else if(diff23 < 2 && diff23 >= 0.25){
      Serial.println("correcting offset as robot moves forward");
      // if it needs to veer to the right, slow right wheels down
      if(d2 > d3){
        motors_forward_correction(FORWARD_3INCHES_DELAY, FORWARD_3INCHES_SPEED, 128, 64, 128, 64);
      }
      // else it will need to veer to the left
      else{
        motors_forward_correction(FORWARD_3INCHES_DELAY, FORWARD_3INCHES_SPEED, 64, 128, 64, 128);
      }
    }
                
    // if parallel to wall, don't adjust and just move forward
    else{
      Serial.println("no correction needed, moving forward");
      motors_forward(FORWARD_3INCHES_DELAY,FORWARD_3INCHES_SPEED);
    }
  }
	
  //////////////////////////
  //check if there is space to the right of the robot
  //////////////////////////
  	
  //get right side sensors
  d2 = us2.getDistance(usesCm);
  d3 = us3.getDistance(usesCm);
  
  
  //Measure distance from US2/3 to wall when US5/6 are up against wall
  while(d2 > maxWallDistance && d2 > (d3 + maxWallDistance)){
    //if you got to here in the code,
    //you are at a corner: meaning us2 can see into a room but us3 has a wall in front of it
    cornerWallDetected = true;
  
    //robot inches forward until past the wall
    cornerWallDistance = d3;
  		
    //robot inches forward
    motors_forward(FORWARD_1INCHES_DELAY,FORWARD_3INCHES_SPEED);
    
    //get right side sensors
    delay(60);
    d2 = us2.getDistance(usesCm);
    d3 = us3.getDistance(usesCm);
    	
  }
  
  //if robot just got done inching forward to get past a wall, inch forward a little more one more time
  if(cornerWallDetected){
    //robot inches forward
    motors_forward(FORWARD_1INCHES_DELAY,FORWARD_3INCHES_SPEED);
    cornerWallDetected = false;
  }
  
  // now both d2 and d3 are past the corner
  if(d2 > maxWallDistance && d3 > maxWallDistance){
    //if you got to here in the code,
    //there is empty space to the right of the robot
    turn_right(NINETY_DEGREE_DELAY, NINETY_DEGREE_SPEED);
    motors_forward((cornerWallDistance * UNIT_DISTANCE_DELAY) + (ROBOT_LENGTH * UNIT_DISTANCE_DELAY), FORWARD_3INCHES_SPEED);
  	
    //NOTE:
    /*
    this portion of the code can be reached during a double corner, in which case, both d2 and d3 COULD be very large.
    If that is the case, it will turn right and go forward with the same distance that was used on the last corner
    */
  }

}
//------------------------------------------------------------------------------------------
/**
set the "front" face of the robot
*/
void calibratePos(){
  if(frontFacing){
    //assign default ultrasonic sensors
    //us1 ---> (30,31) etc.
  }
  else{
    //scan sensors
		
    //assign us1-us6 based on info
		
    //turn robot so that it is lengthwise in the hallway
  }
  
}
//------------------------------------------------------------------------------------------
/**
turns the motors on, sets the speed
the left motors (m1 and m3) are slower to account for the robot
veering slightly when it moves forward in a straight line
*/
void set_speed(int motorspeed){
  myMotor1->setSpeed(motorspeed+7);
  myMotor2->setSpeed(motorspeed);
  myMotor3->setSpeed(motorspeed+7);
  myMotor4->setSpeed(motorspeed);
}
//------------------------------------------------------------------------------------------
void motors_forward(int delayTime, int motorSpeed){
  Serial.print("going forward...");
  
  myMotor1->run(FORWARD);
  myMotor2->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor4->run(FORWARD);

  set_speed(motorSpeed);
  delay(delayTime);
  motors_stop();
}

//------------------------------------------------------------------------------------------
/**
this function is used for the robot to veer either left or right to incorporate angle correction
this can be used to have one side of the robot go forward slower than the other side
*/
void motors_forward_correction(int delayTime, int motorspeed, int speed1, int speed2, int speed3, int speed4){
  Serial.print("going forward...");
  
  myMotor1->run(FORWARD);
  myMotor2->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor4->run(FORWARD);
        
  myMotor1->setSpeed(motorspeed+speed1-7);
  myMotor2->setSpeed(motorspeed+speed2);
  myMotor3->setSpeed(motorspeed+speed3-7);
  myMotor4->setSpeed(motorspeed+speed4);
        
  delay(delayTime);
  motors_stop();
}
//------------------------------------------------------------------------------------------
/**
this function will take readings from us2 and us3, and will
adjust the robot to be parallel to the wall to the right of it
*/
void angle_correction(){
  
  delay(60);
  d2 = us2.getDistance(usesCm);
  d3 = us3.getDistance(usesCm);
  
  diff23 = d2 - d3;
  diff23 = abs(diff23);
  // while the sensor difference is greater than a certain amount, the robot will
  // make minor adjustments to be flush with the wall
  while(diff23 > 0.2){
    if(d2 > d3){
      turn_right(45, 128);
    }
    else{
      turn_left(45, 128);
    }
    
    delay(60);
    d2 = us2.getDistance(usesCm);
    d3 = us3.getDistance(usesCm);
    diff23 = d2 - d3;
    diff23 = abs(diff23);
  }
  Serial.println("Angle corrected in function");
}
//------------------------------------------------------------------------------------------
void motors_backward(int delayTime, int motorSpeed){
  Serial.print("moving back...");
  
  myMotor1->run(BACKWARD);
  myMotor2->run(BACKWARD);
  myMotor3->run(BACKWARD);
  myMotor4->run(BACKWARD);
  
  set_speed(motorSpeed);
  delay(delayTime);
  motors_stop();
}
//------------------------------------------------------------------------------------------
void turn_left(int delayTime, int motorSpeed){
  Serial.print("turning left...");
        
  myMotor1->run(FORWARD);
  myMotor2->run(BACKWARD);
  myMotor3->run(BACKWARD);
  myMotor4->run(FORWARD);
  
  set_speed(motorSpeed);
  delay(delayTime);
  motors_stop();
}
//------------------------------------------------------------------------------------------
void turn_right(int delayTime, int motorSpeed){
  Serial.print("turning right...");
  
  myMotor1->run(BACKWARD);
  myMotor2->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor4->run(BACKWARD);
  
  set_speed(motorSpeed);
  delay(delayTime);
  motors_stop();
}
//------------------------------------------------------------------------------------------
/**
stops the motors
*/
void motors_stop(){
  Serial.print("STOP.");
  
  myMotor1->run(RELEASE);
  myMotor2->run(RELEASE);
  myMotor3->run(RELEASE);
  myMotor4->run(RELEASE);
  
  delay(500);
}
